function [angleObj,angleGoal]=calcuAngle(objNumber,objMat,positionNow,goalPoint)

for k1=1:objNumber
    deltaX = objMat(k1,1)-positionNow(1);
    deltaY = objMat(k1,2)-positionNow(2);
    deltaZ = objMat(k1,3)-positionNow(3);
    
    r=sqrt(deltaX^2+deltaY^2+deltaZ^2);
    fai=atan2(deltaY,deltaX);
    theta=acos(deltaZ/r);
    
    x=sin(theta)*cos(fai);
    y=sin(theta)*sin(fai);
    z=cos(theta);
    
    angleObj(1,k1)=x;
    angleObj(2,k1)=y;
    angleObj(3,k1)=z;
end
    deltaX = goalPoint(1)-positionNow(1);
    deltaY = goalPoint(2)-positionNow(2);
    deltaZ = goalPoint(3)-positionNow(3);
    
    r=sqrt(deltaX^2+deltaY^2+deltaZ^2);
    fai=atan2(deltaY,deltaX);
    theta=acos(deltaZ/r);
    
    x=sin(theta)*cos(fai);
    y=sin(theta)*sin(fai);
    z=cos(theta);
    
    angleGoal(1)=x;
    angleGoal(2)=y;
    angleGoal(3)=z;
end
    
